/**
 * Programme pour le mini Segway
 * Crée par ROHOU V. & THETIOT M.
 */
 

#include <Servo.h> 

//*****************************************
//**** Declaration des entrées/sorties ****
//*****************************************

//Liste des entrées de l'accéléromètre
int pin_Accelero_X = 0;
int pin_Accelero_Y = 1;
int pin_Accelero_Z = 2;

//Sortie Moteur
int pin_Motor = 10;

//Controles logiques du système
int pin_SetNeutral  = 5;

//*****************************************
//******* Declaration des variables *******
//*****************************************

//Tableau d'echantillonage des valeurs d'accelero
int tab[500];

//Constante pour choisir l'axe de controle
char controlAxisParameter;

//Valeur de l'axe de controle
int controlAxisValue;

//Valeurs des axes de l'accéléromètre
int value_Accelero_X = 0;
int value_Accelero_Y = 0;
int value_Accelero_Z = 0;

//Valeur de la position neutre;
int value_Neutral = 0;

Servo motor;


void setup() {
  //Choix de l'axe
  controlAxisParameter = 'Z';
  
  //Entrées
  pinMode(pin_SetNeutral, INPUT);
  
  //Sorties
  pinMode(13, OUTPUT);  
    
  motor.attach(pin_Motor);
}

void loop() {
  //Lecture des trois axes
  value_Accelero_X = analogRead(pin_Accelero_X);
  value_Accelero_Y = analogRead(pin_Accelero_Y);
  value_Accelero_Z = analogRead(pin_Accelero_Z);
  
  //Mémorisation de l'axe de controle
  if(controlAxisParameter ==  'X')
    controlAxisValue = value_Accelero_X;
  if(controlAxisParameter ==  'Y')
    controlAxisValue = value_Accelero_Y;
  if(controlAxisParameter ==  'Z')
    controlAxisValue = value_Accelero_Z;
   
  int value_Neutral; 
    
  //Si l'utilisateur veut mémoriser une nouvelle valeur neutre d'inclinaison  
  if(digitalRead(pin_SetNeutral) == HIGH) {
    value_Neutral = controlAxisValue;    
    digitalWrite(13, HIGH);
  }
  else {
    digitalWrite(13, LOW);
  }
  
  //Calcul de l'écart avec la valeur neutre
  int delta = value_Neutral - controlAxisValue;
  
  int valueMoteur = 90;
  
  if(delta < 0) {
    valueMoteur = map(delta, 0, value_Neutral, 50, 90);
  }
  else {
    valueMoteur = map(delta, value_Neutral, 500, 90, 105);
  }
  
  motor.write(valueMoteur);
  
  
}

void initializeTab() {
  int i;
  int pas = 10;
  int currentStep = 0;
  
  for(i=0; i<500;i++) {
    if(i%pas == 1) {
      currentStep++;
    }
    
    tab[i] = currentStep;
  }
}
